#include <sys/task.h>
#include <stdbool.h>
#include <sys/printk.h>
#include <yonex/debug.h>

#define PAGE_SIZE   0x1000

extern void task_switch(task_t *next);
task_t *a = (task_t *)0x1000;
task_t *b = (task_t *)0x2000;

task_t *running_task()
{
    task_t *task;
    asm volatile(
        "movq %%rsp, %0\n"
        "andq $0xfffffffffffff000, %0\n"
        : "=r"(task)
    );
    return task;
}

void schedule()
{
    task_t *current = running_task();
    task_t *next = (current == a) ? b : a;
    task_switch(next);
}

static void task_create(task_t *task, target_t target)
{
    u64 stack = (u64)task + PAGE_SIZE;
    stack -= sizeof(task_frame_t);
    task_frame_t *frame = (task_frame_t *)stack;
    frame->rbx = 0x11111111;
    frame->rbp = 0x22222222;
    frame->r12 = 0x33333333;
    frame->r13 = 0x44444444;
    frame->r14 = 0x55555555;
    frame->r15 = 0x66666666;
    frame->eip = (void *)target;

    task->stack = stack;
}

void __attribute__((naked)) task_a()
{
    asm volatile("sti\n");
    while (true) {
        printk("a");
        // schedule();
    }
}

void __attribute__((naked)) task_b()
{
    asm volatile("sti\n");
    while (true) {
        printk("b");
        // schedule();
    }
}

void task_init()
{
    task_create(a, (u64)task_a);
    task_create(b, (u64)task_b);
    // schedule();
}
